
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_icengine.c
  * @author     baiyang
  * @date       2022-12-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_icengine.h"

#include <stdio.h>
#include <string.h>

#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_icengine_ctor(sim_icengine_t icengine, uint8_t _throttle, int8_t _choke, int8_t _ignition, int8_t _starter, float _slew_rate, bool _throttle_reversed)
{
    memset(icengine, 0, sizeof(struct sim_icengine));

    icengine->throttle_servo = _throttle;
    icengine->choke_servo = _choke;
    icengine->ignition_servo = _ignition;
    icengine->starter_servo = _starter;
    icengine->slew_rate = _slew_rate;
    icengine->throttle_reversed = _throttle_reversed;
}

/*
  update engine state, returning power output from 0 to 1
 */
float sim_icengine_update(sim_icengine_t icengine, const struct sitl_input *input)
{
    bool have_ignition = icengine->ignition_servo>=0;
    bool have_choke = icengine->choke_servo>=0;
    bool have_starter = icengine->starter_servo>=0;
    float throttle_demand = (input->servos[icengine->throttle_servo]-1000) * 0.001f;
    if (icengine->throttle_reversed) {
        throttle_demand = 1 - throttle_demand;
    }

    icengine->state.ignition = have_ignition?input->servos[icengine->ignition_servo]>1700:true;
    icengine->state.choke = have_choke?input->servos[icengine->choke_servo]>1700:false;
    icengine->state.starter = have_starter?input->servos[icengine->starter_servo]>1700:false;

    uint64_t now = time_micros64();
    float dt = (now - icengine->last_update_us) * 1.0e-6f;
    float max_change = icengine->slew_rate * 0.01f * dt;
    
    if (!have_starter) {
        // always on
        icengine->last_output = throttle_demand;
        return icengine->last_output;
    }

    if (icengine->state.value != icengine->last_state.value) {
        printf("choke:%u starter:%u ignition:%u\n",
               (unsigned)icengine->state.choke,
               (unsigned)icengine->state.starter,
               (unsigned)icengine->state.ignition);
    }
    
    if (have_ignition && !icengine->state.ignition) {
        // engine is off
        if (!icengine->state.starter) {
            goto engine_off;
        }
        // give 10% when on starter alone without ignition
        icengine->last_update_us = now;
        throttle_demand = 0.1f;
        goto output;
    }
    if (have_choke && icengine->state.choke && now - icengine->start_time_us > 1000*1000UL) {
        // engine is choked, only run for 1s
        goto engine_off;
    }
    if (icengine->last_output <= 0 && !icengine->state.starter) {
        // not started
        goto engine_off;
    }
    if (icengine->start_time_us == 0 && icengine->state.starter) {
        if (throttle_demand > 0.2f) {
            printf("too much throttle to start: %.2f\n", throttle_demand);
        } else {
            // start the motor
            if (icengine->start_time_us == 0) {
                printf("Engine started\n");
            }
            icengine->start_time_us = now;
        }
    }
    if (icengine->start_time_us != 0 && icengine->state.starter) {
        uint32_t starter_time_us = (now - icengine->start_time_us);
        if (starter_time_us > 3000*1000UL && !icengine->overheat) {
            icengine->overheat = true;
            printf("Starter overheat\n");            
        }
    } else {
        icengine->overheat = false;
    }

output:
    if (icengine->start_time_us != 0 && throttle_demand < 0.01f) {
        // even idling it gives some thrust
        throttle_demand = 0.01f;
    }

    icengine->last_output = math_constrain_float(throttle_demand, icengine->last_output-max_change, icengine->last_output+max_change);
    icengine->last_output = math_constrain_float(icengine->last_output, 0, 1);

    icengine->last_update_us = now;
    icengine->last_state = icengine->state;
    return icengine->last_output;

engine_off:
    if (icengine->start_time_us != 0) {
        printf("Engine stopped\n");
    }
    icengine->last_update_us = time_micros64();
    icengine->start_time_us = 0;
    icengine->last_output = 0;
    icengine->last_state = icengine->state;
    icengine->start_time_us = 0;
    return 0;
}
/*------------------------------------test------------------------------------*/


